#include "RobotBase.h"

#include <sstream>

using namespace std;

RobotBase::RobotBase(Model* model)
{
    if(NULL == model)
    {
        cout<<"!!!Can't initalize robot class, model is null!!!\n";
        return;
    }

    // Initialize movement control parameter
    avoidcount = 0;
    randcount = 0;

    verbose = false;
    processMsg = false;
    communication = 0;

    // Initialize devices
    position = (ModelPosition*) model;
    laser = (ModelLaser*) model->GetChild( "laser:0" );
    wifi = (ModelWifi*) model->GetChild("wifi:0");
    
    id = wifi->GetId()/4 - 1;
    Log("Robot has been initalized");
    GroupSize = 0;

    for(int i = 0; i < VerboseNumber; i++)
    {
        if(VerboseList[i] == id)
            verbose = true;
    }
    simData = new SimulationData(id);
    this->startx = this->position->GetGlobalPose().x;
    movementType = 0;
}

void RobotBase::DoLoop()
{
    // Log the simualtion data

    // Get simulation time and convert its unit to seconds
    unsigned int timeNow = GetTime()/simData->SimPeriod;
    
    if (simData->lastLogTime != timeNow)
    {
        simData->LogWithTime(id, GroupSize, communication, timeNow);
    }
    
    switch(this->movementType)
    {
        case 0:
            break;
        case 1:
            Wander();
            break;
        case 2:
            MoveStraight();
            break;
        default:
            printf("Fuck!!! What's the hell movement type you give me!!!\n");
            break;
    }
    // Estimate the group size
    Estimate();
}

void RobotBase::Broadcast(WifiMessageBase* msg)
{
    wifi->comm.SendBroadcastMessage(msg);
    communication++;
}

unsigned int RobotBase::GetTime()
{
    return (unsigned int)position->GetWorld()->SimTimeNow();
}

void RobotBase::Log(string logMsg, LogType type)
{
    if(type == DebugLog && verbose == false)
        return;
    switch(type)
    {
        case ErrorLog:
            cout<<"[Error]";
            break;
        case DebugLog:
            cout<<"[Debug]";
            break;
        case NormalLog:
            cout<<"[Log]";
            break;
    }
    cout<<"[Robot "<<this->id<<"]";
    cout<<"[Time "<<GetTime()<<"]";
    cout<<logMsg<<endl;
}

void RobotBase::Sleep(unsigned int sec)
{
}

void RobotBase::MoveStraight()
{
    const double cruisespeed = 0.2; 

    //if(this->position->GetGlobalPose().x - this->startx < MaxMove && GetTime() > 40000000)
    if(this->position->GetGlobalPose().x - this->startx < MaxMove && GetTime() > 40000000)
    {
        position->SetXSpeed( cruisespeed );	  
        position->SetTurnSpeed(  0 );
    }
    else
    {
        position->SetXSpeed( 0 );	  
        position->SetTurnSpeed(  0 );
    }
}

int RobotBase::Wander()
{
    const double cruisespeed = 0.2; 
    const double avoidspeed = 0.1; 
    const double avoidturn = 1;
    const double minfrontdistance = 2.0; // 0.6  
    const double stopdist = 0.7;
    const int avoidduration = 10;

    //get the data
    uint32_t sample_count = 0;
    ModelLaser::Sample* scan = this->laser->GetSamples( &sample_count);
    if( ! scan )
        return 0;

    bool obstruction = false;
    bool stop = false;

    //find the closest distance to the left and right and check if there's anything in front
    double minleft = 1e6;
    double minright = 1e6;

    for( uint32_t i = 0; i < sample_count; i ++)
    {
        if( (i > (sample_count/3)) && (i < (sample_count - (sample_count/3))) && scan[i].range < minfrontdistance)
        {
            obstruction = true;
        }

        if( scan[i].range < stopdist )
        {
            stop = true;
        }

        if( i > sample_count/2 )
            minleft = std::min( minleft, scan[i].range );
        else      
            minright = std::min( minright, scan[i].range );

    }

    if( obstruction || stop || ( avoidcount > 0) )
    {
        position->SetXSpeed( stop ? 0.0 : avoidspeed );      

        /* once we start avoiding, select a turn direction and stick with it for a few iterations */
        if( avoidcount < 1 )
        {
            avoidcount = random() % avoidduration + avoidduration;

            if( minleft < minright  )
            {
                position->SetTurnSpeed( -avoidturn );
            }
            else
            {
                position->SetTurnSpeed( +avoidturn );
            }
        }

        avoidcount--;
    }
    else
    {
        avoidcount = 0;
        position->SetXSpeed( cruisespeed );	  
        position->SetTurnSpeed(  0 );
    }
    return 0;
}// end Wander


